#include "SDFManager.h"
#include <iostream>
#include <FileSystem.h>
#include <QStringList>
#include <QFileInfo>
#include <QDir>
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osg/Geode>
#include <SDFDofRange.h>
#include <PhysicsWorldFunctions.h>
#include <PhysicsWorld.h>

SDFManager* SDFManager::_instance = NULL;

SDFManager::SDFManager()
{
    m_contactDebugDrawerNode = new osg::Group();
}

SDFManager::~SDFManager()
{

}

osg::ref_ptr<SDF> SDFManager::getSDF(int ptrInt)
{
    osg::ref_ptr<SDF> sdfPtr;
    sdfPtr = NULL;
    for(int i = 0 ; i < m_sdfVector.size(); i++)
    {
        if(m_sdfVector[i]->getRigidPtrInt() == ptrInt)
        {
            sdfPtr = m_sdfVector[i];
            break;
        }
    }

    return sdfPtr;
}

void SDFManager::readFromFile(const std::string &fileName)
{
    //std::cout<<"SDFManager::readFromFile fileName="<<fileName<<std::endl;

    FileSystem::instance()->loadFile(QString::fromStdString(fileName));
    QDomElement elementDoc = FileSystem::instance()->getDocDomElement();
    for(int i = 0;i<elementDoc.childNodes().size(); i++)
    {
        QDomElement elementSDFInfo = elementDoc.childNodes().at(i).toElement();
        int intPtr = elementSDFInfo.attribute("intPtr").toInt();
        //std::cout<<"SDFManager::readFromFile intPtr="<<intPtr<<std::endl;

        osg::ref_ptr<SDF> theSDF = new SDF();
        m_sdfVector.push_back(theSDF);
        theSDF->setRigidPtrInt(intPtr);
        theSDF->setOsgNodeName(elementSDFInfo.attribute("nodeName").toStdString());
        //connect ptr and ptrActual
        int ptrActual =(int)( PhysicsWorldFunctions::findRigidBodyByNodeName( elementSDFInfo.attribute("nodeName").toStdString() ) );
        this->addPtrActualToptr(ptrActual, intPtr);
        theSDF->setRigidPtrIntActual(ptrActual);
        //read grid info
        {
            QFileInfo fileInfoGrid(QString::fromStdString(fileName));
            QString gridFilePath = fileInfoGrid.absoluteDir().path()+"/"+elementSDFInfo.attribute("intPtr") + ".grid";
            std::cout<<"SDFManager::readFromFile gridFilePath="<<gridFilePath.toStdString()<<std::endl;

            FileSystem::instance()->loadFile(gridFilePath);
            QDomElement elementDocGrid = FileSystem::instance()->getDocDomElement();
            QDomElement elementGridInfo = elementDocGrid.childNodes().at(0).toElement();
            osg::Vec3 vec = strToVec3(elementGridInfo.attribute("minCoordinate"));
            theSDF->setMinCoordinate(vec.x(), vec.y(), vec.z());

            vec = strToVec3(elementGridInfo.attribute("nodeCountVector"));
            theSDF->setNodeCountVector(vec.x(), vec.y(), vec.z());

            //theSDF.setRigidPtrInt(elementGridInfo.attribute("ptr").toInt());

            theSDF->setNodeCount(elementSDFInfo.attribute("nodeCount").toInt());

            vec = strToVec3(elementGridInfo.attribute("dxdydz"));
            theSDF->setdxdydz(vec.x(), vec.y(), vec.z());

            vec = strToVec3(elementGridInfo.attribute("maxCoordinate"));
            theSDF->setMaxCoordinate(vec.x(), vec.y(), vec.z());

            //read binary file:
            QString gridValueFilePath = fileInfoGrid.absoluteDir().path()+"/"+elementSDFInfo.attribute("intPtr") + ".gridData";
            FileSystem::instance()->readBinaryFile(gridValueFilePath, theSDF->getGridValue());
        }

        //read sample point info
        {
            QFileInfo fileInfoGrid(QString::fromStdString(fileName));
            QString sampleFilePath = fileInfoGrid.absoluteDir().path()+"/"+elementSDFInfo.attribute("intPtr") + ".samplePoints";
            //std::cout<<"SDFManager::readFromFile sampleFilePath="<<sampleFilePath.toStdString()<<std::endl;

            FileSystem::instance()->loadFile(sampleFilePath);
            QDomElement elementDocSample = FileSystem::instance()->getDocDomElement();
            QDomElement elementSampleInfo = elementDocSample.childNodes().at(1).toElement();

            theSDF->setPointCount(elementSampleInfo.attribute("PointCount").toInt());

            //read binary file:
            QString sampleValueFilePath = fileInfoGrid.absoluteDir().path()+"/"+elementSDFInfo.attribute("intPtr") + ".sampleData";
            std::cout<<"SDFManager::readFromFile sampleFilePath="<<sampleValueFilePath.toStdString()<<std::endl;
            FileSystem::instance()->readBinaryFile(sampleValueFilePath, theSDF->getSampleValue());
        }

    }
}

void SDFManager::addPtrActualToptr(int ptrActual, int ptr)
{
    m_ptrActualToptr[ptrActual] = ptr;
}

int SDFManager::getPtr(int ptrActual)
{
    return m_ptrActualToptr[ptrActual];
}

void SDFManager::clearSDFContacts()
{
    m_sdfContacts.clear();
}

void SDFManager::addSDFContact(osg::ref_ptr<SDFContact> sdfContact)
{
    m_sdfContacts.push_back(sdfContact);
}

int SDFManager::getSDFContactsCount()
{
    return m_sdfContacts.size();
}

osg::ref_ptr<osg::Node> SDFManager::getContactDebugDrawerNode()
{
    return m_contactDebugDrawerNode;
}

void SDFManager::updateContactDebugDrawerNode()
{
    //std::cout<<"SDFManager::updateContactDebugDrawerNode contacts="<<m_sdfContacts.size()<<std::endl;
m_contactDebugDrawerNode->removeChildren(0, m_contactDebugDrawerNode->getNumChildren());
    osg::ref_ptr<osg::Vec3Array> m_pointVector;
    osg::ref_ptr<osg::Geometry> m_geom;
    osg::ref_ptr<osg::LineWidth> m_lineWidth;
    osg::ref_ptr<osg::Vec4Array> m_colorArray;

    //add coordinate
    m_geom = new osg::Geometry;
    m_pointVector = new osg::Vec3Array;
    m_colorArray = new osg::Vec4Array;
    osg::ref_ptr<osg::Vec3Array> normalArray = new osg::Vec3Array;

    m_geom->setVertexArray( m_pointVector.get() );
    m_geom->setColorArray( m_colorArray.get() );
    m_geom->setColorBinding( osg::Geometry::BIND_OVERALL );
    m_geom->setNormalArray( normalArray.get() );
    m_geom->setNormalBinding( osg::Geometry::BIND_OVERALL );

    normalArray->push_back( osg::Vec3( 0.f,  0.f, 1.f ) );

    //set data
    m_colorArray->push_back(osg::Vec4(1,0,0,1));

    //point
    //m_pointVector->push_back(osg::Vec3(0,0,0));
    //m_pointVector->push_back(osg::Vec3(1, 1, 1));
    for(int i = 0; i < m_sdfContacts.size(); i++)
    {
        for(int j = 0; j < m_sdfContacts[i]->getContactPointsCount(); j++ )
        {
            osg::Vec3 pos = m_sdfContacts[i]->getContactPosByIndex(j);
            osg::Vec3 normal = m_sdfContacts[i]->getContactNormalByIndex(j);
            normal.normalize();
            normal = normal*0.01;
            m_pointVector->push_back(pos);
            m_pointVector->push_back(pos + normal);
        }
    }


    m_geom->addPrimitiveSet(new osg::DrawArrays( osg::PrimitiveSet::LINES, 0,  m_pointVector->size() ) );

    //line-size
    m_lineWidth = new osg::LineWidth;
    m_lineWidth ->setWidth(2);
    m_geom->getOrCreateStateSet()->setAttributeAndModes(m_lineWidth.get (),osg::StateAttribute::ON);

    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
    geode->addDrawable(m_geom);
    //geode->setNodeMask(0xfffffffe);//geode will not show on objectInspector
    m_contactDebugDrawerNode->addChild(geode);
    //disable light of coordinate
    osg::StateSet* stateset = geode->getOrCreateStateSet();
    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);//这里GL_LIGHTING是系统opengl的头文件GL.h中定义的

}


void SDFManager::computeSDFDofAll()
{
    for(int i = 0; i < m_sdfVector.size(); i++)//
    {
        //if(m_sdfVector.at(i)->get)
        computeSDFDOf(m_sdfVector.at(i));
    }

    for(int i = 0; i < m_sdfVector.size(); i++)
    {
        osg::ref_ptr<SDF> sdfPtr = m_sdfVector[i];
        if(sdfPtr->getSDFDof()->getDofCount() > 0 && sdfPtr->getSDFDof()->getDofCount() < 6)
        {
            std::cout<<"SDFManager::computeSDFDofAll "<<sdfPtr->getOsgNodeName()<<std::endl;
            sdfPtr->getSDFDof()->printAxisSignal();
        }
    }
}

void SDFManager::computeDisassemblySequenceAll()
{
    recoveAllSDFCollisionEnable();
    PhysicsWorld::instance()->performCollisionOnce();
    computeSDFDofAll();
    m_disassemblySequence.push_back(computeDisassemblySequence());
    while(m_disassemblySequence.at(m_disassemblySequence.size()-1).size()>0)
    {
        PhysicsWorld::instance()->performCollisionOnce();
        computeSDFDofAll();
        m_disassemblySequence.push_back(computeDisassemblySequence());
    }
    printDisassemblySequence();
}

void SDFManager::computeSDFDOf(osg::ref_ptr<SDF> sdfPtr)
{
    osg::ref_ptr<SDFDof> dofPtr = sdfPtr->getSDFDof();
    //get axis
    osg::Vec3 xAxis = dofPtr->getXAxisInWorldFrame();
    osg::Vec3 yAxis = dofPtr->getYAxisInWorldFrame();
    osg::Vec3 zAxis = dofPtr->getZAxisInWorldFrame();

    std::vector<osg::ref_ptr<SDFContact> > contacts = findContactOfSDF(sdfPtr);
    dofPtr->setContacts(contacts);
    dofPtr->computeAxis();
    bool xAxis1 = true;bool xAxis2 = true;
    bool yAxis1 = true;bool yAxis2 = true;
    bool zAxis1 = true;bool zAxis2 = true;
    for(int i = 0; i < contacts.size(); i++)
    {
        osg::ref_ptr<SDFContact> contactPtr = contacts.at(i);

        xAxis1 &= contactPtr->chargeDirectionIsValid(sdfPtr,xAxis);
        xAxis2 &= contactPtr->chargeDirectionIsValid(sdfPtr,xAxis*-1.0);
        yAxis1 &= contactPtr->chargeDirectionIsValid(sdfPtr,yAxis);
        yAxis2 &= contactPtr->chargeDirectionIsValid(sdfPtr,yAxis*-1.0);
        zAxis1 &= contactPtr->chargeDirectionIsValid(sdfPtr,zAxis);
        zAxis2 &= contactPtr->chargeDirectionIsValid(sdfPtr,zAxis*-1.0);
    }
    std::vector<bool> m_axisSignal;
    m_axisSignal.push_back(xAxis1);m_axisSignal.push_back(xAxis2);
    m_axisSignal.push_back(yAxis1);m_axisSignal.push_back(yAxis2);
    m_axisSignal.push_back(zAxis1);m_axisSignal.push_back(zAxis2);
    dofPtr->setAxisSignal(m_axisSignal);
}

void SDFManager::printContacts()
{
    for(int i=0; i< m_sdfContacts.size(); i++)
    {
        std::string name1 = m_sdfContacts.at(i)->getSDF1()->getOsgNodeName();
        std::string name2 = m_sdfContacts.at(i)->getSDF2()->getOsgNodeName();

        std::cout<<"SDFManager "<<name1<<"   "<<name2<<std::endl;
    }
}

void SDFManager::saveContactsToMatlabInputFile(osg::ref_ptr<SDF> sdfPtr)
{
    std::vector<osg::ref_ptr<SDFContact>> contacts = this->findContactOfSDF(sdfPtr);
    std::vector<osg::Vec3> normals;
    for(int i=0; i<contacts.size(); i++)
    {
        double signal = 1.0;
        if(sdfPtr.get() == contacts.at(i)->getSDF1())
            signal = -1.0;
        for(int j = 0; j < contacts.at(i)->getContactPointsCount(); j++)
        {
            normals.push_back(contacts.at(i)->getContactNormalByIndex(j) * signal);
        }
    }
    FileSystem::instance()->saveVec3VectorToMatlabFile("tempFiles/MatlabFile/A.txt",normals);
    FileSystem::instance()->saveVec3VectorToMatlabFile2("tempFiles/MatlabFile/b.txt",normals.size());
}

std::vector<osg::ref_ptr<SDFContact> > SDFManager::findContactOfSDF(osg::ref_ptr<SDF> sdfPtr)
{
    std::vector<osg::ref_ptr<SDFContact> > findedContacts;
    for(int i = 0 ; i < m_sdfContacts.size(); i++)
    {
        SDF* sdf1 = m_sdfContacts[i]->getSDF1();
        SDF* sdf2 = m_sdfContacts[i]->getSDF2();
        if(sdf1 == sdfPtr.get() || sdf2 == sdfPtr.get())
        {
            findedContacts.push_back(m_sdfContacts[i]);
        }
    }

    return findedContacts;
}

QStringList SDFManager::computeDisassemblySequence()
{
    QStringList sequence;
    for(int i = 0 ; i < m_sdfVector.size(); i++)
    {
        osg::ref_ptr<SDF> sdfPtr = m_sdfVector[i];
        if(sdfPtr->getCollisionEnable() == false)
        {
            continue;
        }
        osg::ref_ptr<SDFDof> dofPtr = sdfPtr->getSDFDof();
        int dofCount = dofPtr->getDofCount();
        if(dofCount > 0)
        {
            sequence.push_back(QString::fromStdString(sdfPtr->getOsgNodeName()) );
            sdfPtr->setCollisionEnable(false);
        }
    }
    return sequence;
}

osg::Vec3 SDFManager::strToVec3(const QString &strValue)
{
    osg::Vec3 vec;
    QStringList strList = strValue.split(" ");
    if(strList.size() >= 3)
    {
        vec.set(strList.at(0).toFloat(), strList.at(1).toFloat(),strList.at(2).toFloat());
    }
    return vec;
}

void SDFManager::recoveAllSDFCollisionEnable()
{
    for(int i = 0; i < m_sdfVector.size(); i++)
    {
        m_sdfVector[i]->setCollisionEnable(true);
    }
}

void SDFManager::printDisassemblySequence()
{
    std::cout<<"SDFManager::printDisassemblySequence"<<std::endl;
    for(int i = 0 ; i < m_disassemblySequence.size(); i++)
    {
        QStringList strList = m_disassemblySequence.at(i);
        for(int j = 0; j < strList.size(); j++)
        {
            std::cout<<strList.at(j).toStdString()<<", ";
        }
        std::cout<<std::endl;
    }
}

SDFManager *SDFManager::instance()
{
    if (_instance == NULL)
    {
        _instance = new SDFManager();
    }
    return _instance;

}


